#!/usr/bin/env python
# -*- encoding: utf-8 -*-
"""
@File         :vir_leader_formation_controller.py
@Description  :编队控制器
@Time         :2024/05/23 17:01:34
@Author       :Lin Yuheng
@Version      :1.0
"""

import os
import sys

sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), ".")))
# refer to: https://docs.python-guide.org/writing/structure/

import rospy
from formation_controller import FormationController
from tools.formation_structure import *
from tools.mathlib import *
from tools.vector import *
from tools.filter import *
from tools.increment_pid_controller import *
from tools.pid_controller import *
from tools.params import *
from autopilot.L1_Controller import L1Controller
from autopilot.TECS import TECSController

from mavros_msgs.msg import State
from geometry_msgs.msg import Pose2D


class VirLeaderFormationControllerTest(FormationController):
    def __init__(self):
        super(VirLeaderFormationControllerTest, self).__init__()
        
        self.uavID = "plane_1"
        
        # 绝对速度位置控制器时间戳
        self._abs_pos_vel_ctrl_timestamp = 0.0

        # 滤波后的领机（虚拟领机）信息
        self._leader_states_f = S_FwLeaderStates()
        # 滤波后的本机信息
        self._fw_state_f = S_FwStates()

        # 是否使用滤波器对于原始数据滤波
        self._use_the_filter = True

        # 检验计算本机的空速（状态）以及实际读取的空速的合法性
        self._fw_airspd_states_vaild = True
        # 检验计算领机的空速（状态）以及实际读取的空速的合法性
        self._led_airspd_states_vaild = True

        # 领机空速向量
        self._led_airsp = Vector()
        # 领机地速向量
        self._led_gspeed_2d = Vector()

        # 本机空速向量
        self._fw_airspd = Vector()
        # 本机地速向量
        self._fw_gspeed_2d = Vector()
        # 本机风估计向量
        self._fw_wind_vector = Vector()

        # 领机dir_cos，这其中的dir角度，可能是yaw，也可能是速度偏角
        self._led_cos_dir = 0.0
        # 领机dir_sin，这其中的dir角度，可能是yaw，也可能是速度偏角
        self._led_sin_dir = 0.0
        # 本机dir_cos，这其中的dir角度，可能是yaw，也可能是速度偏角
        self._fw_cos_dir = 0.0
        # 本机dir_sin，这其中的dir角度，可能是yaw，也可能是速度偏角
        self._fw_sin_dir = 0.0

        # 控制器X方向混合误差产生参数
        self._mix_Xerr_params = S_MixXerrParams()
        # 控制器Y方向混合误差产生参数
        self._mix_Yerr_params = S_MixYerrParams()

        # 计算产生期望地速的PID控制器
        self._gspeed_sp_pid = PIDController()

        # 重置内部控制器标志量
        self._rest_speed_pid = False

        # 从机期望地速增量
        self._del_fol_gspeed = 0.0

        # 飞机期望空速
        self._airspeed_sp = 0.0

        # 飞机期望空速（上一时刻）
        self._airspeed_sp_prev = 0.0

        # 本机飞机空速期望值滤波器
        self._airspeed_sp_filter = Filter()

        # （虚拟）领机glo速度x滤波器
        self._led_glo_vel_x_filter = Filter()
        # （虚拟）领机glo速度y滤波器
        self._led_glo_vel_y_filter = Filter()

        """
        TECS函数,变量（组）
        """
        # TECS控制器
        self._tecs = TECSController()

        # 重置TECS控制器标志量
        self._rest_tecs = False

        # 纵向速度有效标志位
        self._vz_valid = False

        # TECS控制器参数
        self._tecs_params = TECSParams()

        """
        L1控制器函数,变量（组）
        """
        # 横侧向控制器
        self._l1_controller = L1Controller()

        # 横侧向控制器参数
        self._lateral_params = LateralControllerParams()

        # 产生期望滚转角的PID控制器
        # self._roll_sp_pid = INCREMENT_PID_CONTROLLER()
        self._roll_sp_pid = PIDController()

        # 当领从角度差别太大，使用速度角来计算期望滚转角
        self._eta_2_roll_sp_pid = PIDController()
        # 重置速度角到滚转PID控制器的标志量
        self._reset_eta_2_roll_sp_pid = False

        # 重置横侧向控制器
        self._reset_lateral_controller = False

        # 最终roll通道控制量
        self._roll_cmd = 0.0

        # 最终roll通道控制量（上一时刻）
        self._roll_cmd_prev = 0.0

        # 期望滚转角滤波器
        self._roll_cmd_filter = Filter()

        # 横纵向混合误差
        self.mix_err_Xk = 0
        self.mix_err_Yk = 0
        
        # 发布横纵向误差话题
        self._mix_err_2D_pub = rospy.Publisher(
            self.uavID + "/fixed_wing_control/mix_err_2D",
            Pose2D,
            queue_size=1,
        )
        
        self._lateral_mode = None

    def fw_states_callback(self, msg: State):
        self.current_state = msg

    def reset_formation_controller(self):
        """
        @Description :重置控制器
        @Param:
        @Returns     :
        """
        self._rest_speed_pid = True
        self._rest_tecs = True

    def set_mix_Xerr_params(self, input_params: S_MixXerrParams):
        """
        @Description :设定编队混合误差产生的参数
        @Param:
        @Returns     :
        """
        self._mix_Xerr_params = input_params

    def set_mix_Yerr_params(self, input_params: S_MixYerrParams):
        """
        @Description :设定滚转角混合误差产生的参数
        @Param:
        @Returns     :
        """
        self._mix_Yerr_params = input_params

    def get_mix_Xerr_params(self):
        """
        @Description :获取编队混合误差产生的参数
        @Param:
        @Returns     :
        """
        return self._mix_Xerr_params

    def get_min_Yerr_params(self):
        """
        @Description :获取滚转角混合误差产生的参数
        @Param:
        @Returns     :
        """
        return self._mix_Yerr_params

    def set_tecs_params(self, input_params: TECSParams):
        """
        @Description :设定tecs控制参数
        @Param:
        @Returns     :
        """
        self._tecs_params = input_params

    def filter_led_fol_states(self):
        self._fw_state_f = self.fw_state
        self._leader_states_f = self.leader_states

        if self._use_the_filter:
            self._leader_states_f.global_vel_n = self._led_glo_vel_x_filter.update(
                self._leader_states_f.global_vel_n
            )
            self._leader_states_f.global_vel_e = self._led_glo_vel_y_filter.update(
                self._leader_states_f.global_vel_e
            )

    def get_plane_to_vector(self, origin: Point, target: Point):
        """
        @Description :获取飞机当前位置到期望位置的指向向量,这里的target和origin均采用的是ned坐标
        @Param:
        @Returns     :
        """
        out = Point(target.x - origin.x, target.y - origin.y)
        return out

    def control_formation(self):
        """
        @Description :控制器主函数
        @Param:
        @Returns     :
        """
        ros_time = rospy.Time.now()
        now = ros_time.to_sec()
        self._dt = constrain(
            now - self._abs_pos_vel_ctrl_timestamp, self._dt_min, self._dt_max
        )

        """
        0. 原始数据滤波，飞机的状态保存在滤波后的状态值里
        """

        self.filter_led_fol_states()

        # if not self.identify_led_fol_states():
        #     rospy.loginfo("警告：领机或从机未在飞行中")
        #     return

        """
        1. 根据队形要求，计算出从机期望位置的在领机航迹坐标系下的位置，然后再到->GPS位置（期望经纬高）；
        在此之中注意领机航迹方向的选择。
          a. 计算领机机头方向
          b. 计算从机期望位置的gps位置
        """
        led_airspd_x = (
            self._leader_states_f.wind_estimate_n + self._leader_states_f.global_vel_n
        )
        led_airspd_y = (
            self._leader_states_f.wind_estimate_e + self._leader_states_f.global_vel_e
        )

        self._led_airsp.set_vec_ele(led_airspd_x, led_airspd_y)
        self._led_gspeed_2d.set_vec_ele(
            self._leader_states_f.global_vel_n, self._leader_states_f.global_vel_e
        )

        # 根据计算空速与读取空速的判断空速状态是否正确
        if self._led_airsp.len() - self._leader_states_f.air_speed >= 3.0:
            self._led_airspd_states_vaild = False
        else: 
            self._led_airspd_states_vaild = True

        # 判断本机地速太小以及航向角是否已知
        if self._led_gspeed_2d.len() <= 3.0:
            if self._leader_states_f.yaw_valid:
                self._led_cos_dir = math.cos(self._leader_states_f.yaw_angle)
                self._led_sin_dir = math.sin(self._leader_states_f.yaw_angle)
                rospy.loginfo("领机地速太小，选用领机航向角")
            else:
                self._led_cos_dir = 0.0
                self._led_sin_dir = 0.0
                rospy.loginfo("领机地速太小，领机航向角未知")
        else:
            # 领机地速正常
            self._led_cos_dir = self._led_gspeed_2d.x / self._led_gspeed_2d.len()
            self._led_sin_dir = self._led_gspeed_2d.y / self._led_gspeed_2d.len()
            rospy.loginfo("领机地速正常，选用领机地速方向")

        # 保证归一化的结果，求领机的速度方向
        led_dir_unit = Vector(self._led_cos_dir, self._led_sin_dir)
        led_dir_unit.normlized()
        # led_dir_unit.print()

        # 计算编队偏差下，从机相对于领机的ned偏差
        self.formation_offset.xb = 0
        self.formation_offset.yb = 0
        self.formation_offset.zb = 0
        
        self.formation_offset.ned_n = (
            self.formation_offset.xb * self._led_cos_dir
            - self.formation_offset.yb * self._led_sin_dir
        )
        self.formation_offset.ned_e = (
            self.formation_offset.xb * self._led_sin_dir
            + self.formation_offset.yb * self._led_cos_dir
        )
        self.formation_offset.ned_d = self.formation_offset.zb

        # TODO 将ned坐标转化为经纬度坐标

        self.fw_sp.ned_pos_n = (
            self.leader_states.ned_pos_n + self.formation_offset.ned_n
        )
        self.fw_sp.ned_pos_e = (
            self.leader_states.ned_pos_e + self.formation_offset.ned_e
        )
        self.fw_sp.ned_pos_d = (
            self.leader_states.ned_pos_d + self.formation_offset.ned_d
        )
        self.fw_sp.altitude = abs(self.fw_sp.ned_pos_d) # TODO 这里直接采用虚拟领导者的ned坐标中的d的绝对值作为altitude

        """
        2. 计算从机期望位置在自身ned坐标下的位置：
          1) 本机地速方向
          2) 本机航迹角
          3) 有误
        """

        fw_airspd_x = self._fw_state_f.wind_estimate_x + self._fw_state_f.global_vel_n
        fw_airspd_y = self._fw_state_f.wind_estimate_y + self._fw_state_f.global_vel_e

        self._fw_airspd.set_vec_ele(fw_airspd_x, fw_airspd_y)
        self._fw_gspeed_2d.set_vec_ele(
            self._fw_state_f.global_vel_n, self._fw_state_f.global_vel_e
        )

        # 根据计算空速与读取空速的判断空速状态是否正确
        if self._fw_gspeed_2d.len() - self.fw_state.air_speed >= 3.0:
            self._fw_airspd_states_vaild = False
        else: 
            self._fw_airspd_states_vaild = True
        # 判断本机地速太小以及航向角是否已知
        if self._fw_gspeed_2d.len() <= 3.0:
            if self._fw_state_f.yaw_valid:
                self._fw_cos_dir = math.cos(self._fw_state_f.yaw_angle)
                self._fw_sin_dir = math.sin(self._fw_state_f.yaw_angle)
                rospy.loginfo("从机地速太小，选用从机航向角")
            else:
                self._fw_cos_dir = 0.0
                self._fw_sin_dir = 0.0
                rospy.loginfo("从机地速太小，且从机航向角未知")
        else:
            # 选用从机地速方向
            self._fw_cos_dir = self._fw_gspeed_2d.x / self._fw_gspeed_2d.len()
            self._fw_sin_dir = self._fw_gspeed_2d.y / self._fw_gspeed_2d.len()

        # 对本机速度进行归一化，求本机速度方向单位向量
        fw_dir_unit = Vector(self._fw_cos_dir, self._fw_sin_dir)
        fw_dir_unit.normlized()

        """
        3. 计算从机期望位置与当前位置的误差在从机航迹系下的投影
        """

        # 用经纬度坐标来表示期望位置，当前是用的是ned坐标
        pos_sp = Point(self.fw_sp.ned_pos_n, self.fw_sp.ned_pos_e)  # 期望位置
        current_pos = Point(
            self._fw_state_f.ned_pos_n, self._fw_state_f.ned_pos_e
        )  # 当前位置
        vec_plane_sp = Vector(
            pos_sp.x - current_pos.x, pos_sp.y - current_pos.y
        )  # 飞机到期望点的向量（误差向量）

        self.fw_error.P_N = vec_plane_sp.x
        self.fw_error.P_E = vec_plane_sp.y
        # self.fw_error.P_D = (self.fw_sp.altitude - self._fw_state_f.altitude)
        self.fw_error.P_D = (self.fw_sp.altitude - abs(self._fw_state_f.ned_pos_d)) # TODO 这里需要再明确一下
        self.fw_error.P_NE = math.sqrt((self.fw_error.P_N**2 + self.fw_error.P_E**2))

        self.fw_error.PXk = led_dir_unit.dot(vec_plane_sp)
        directioon = led_dir_unit.cross(vec_plane_sp)
        error_vert = vec_plane_sp.sub(led_dir_unit.mul(led_dir_unit.dot(vec_plane_sp)))
        if directioon < 0:
            self.fw_error.PYk = - error_vert.len()
        else:
            self.fw_error.PYk = error_vert.len()
        
        
        # self.fw_error.PXk = fw_dir_unit.dot(vec_plane_sp)
        # self.fw_error.PYk = fw_dir_unit.cross(vec_plane_sp)

        """
        4. 计算领机从机地速“差”在从机航迹坐标系之中的投影，控制量是地速，所以是地速之差
        """
        led_fol_vel_error = self._led_gspeed_2d.sub(self._fw_gspeed_2d)
        self.fw_error.led_fol_vxk = fw_dir_unit.dot(led_fol_vel_error)
        self.fw_error.led_fol_vyk = fw_dir_unit.cross(led_fol_vel_error)
        self.fw_error.led_fol_vzk = (
            self._leader_states_f.global_vel_d - self._fw_state_f.global_vel_d
        )

        self.fw_error.led_fol_vk = math.sqrt(
            self.fw_error.led_fol_vxk**2
            + self.fw_error.led_fol_vyk**2
            + self.fw_error.led_fol_vzk**2
        )

        """
        5. 计算领机从机速度角度，机体前向右偏为正,范围(0~pi/2),左偏为负，范围（-pi/2~0)
        """
        # 计算领机从机的速度夹角
        led_gsp_Xk = fw_dir_unit.dot(self._led_gspeed_2d)
        led_gsp_Yk = fw_dir_unit.cross(self._led_gspeed_2d)

        if led_gsp_Xk > 0 and led_gsp_Yk > 0: # 右前方
            self.fw_error.led_fol_eta = math.atan(led_gsp_Yk / led_gsp_Xk)
        elif led_gsp_Xk < 0 and led_gsp_Yk > 0: # 右后方
            self.fw_error.led_fol_eta = math.pi + math.atan(led_gsp_Yk / led_gsp_Xk)
        elif led_gsp_Xk > 0 and led_gsp_Yk < 0: # 左前方
            self.fw_error.led_fol_eta = math.atan(led_gsp_Yk / led_gsp_Xk)
        elif led_gsp_Xk < 0 and led_gsp_Yk < 0: # 左后方
            self.fw_error.led_fol_eta = - math.pi + math.atan(led_gsp_Yk / led_gsp_Xk)
        elif led_gsp_Xk == 0 and led_gsp_Yk < 0: # 正左方
            self.fw_error.led_fol_eta = - math.pi / 2
        elif led_gsp_Xk == 0 and led_gsp_Yk > 0: # 正右方
            self.fw_error.led_fol_eta = math.pi / 2
        elif led_gsp_Yk == 0 and led_gsp_Xk < 0: # 正后方
            self.fw_error.led_fol_eta = - math.pi
        elif led_gsp_Yk == 0 and led_gsp_Xk > 0: # 正前方
            self.fw_error.led_fol_eta = 0
        else: # 速度方向有误
            self.fw_error.led_fol_eta = 0
            rospy.loginfo("领机从机速度夹角有误")

        """
        6. 判断控制分段
        """

        if self.fw_error.PXk > 60.0:
            self.__controller_method = LONG_DIS
            rospy.loginfo("控制分段：远距离")
        else:
            self.__controller_method = CLOSE_DIS
            rospy.loginfo("控制分段：近距离")

        """
        7. 根据前向位置误差，产生期望速度大小
        """

        # if self.__controller_method == LONG_DIS:
        if False:
            self.fw_sp.air_speed = self.fw_params.max_airspeed_sp
            self._gspeed_sp_pid.set_prev_output()
        # elif self.__controller_method == CLOSE_DIS:
        elif True:
            # 1.产生前向混合误差
            self.mix_err_Xk = (
                self._mix_Xerr_params.kp_p * self.fw_error.PXk
                + self._mix_Xerr_params.kv_p * self.fw_error.led_fol_vk
            )
            # rospy.loginfo("self.fw_error.PXK: {}".format(self.fw_error.PXk))
            # rospy.loginfo("前向混合误差: {}".format(self.mix_err_Xk))
            # 2.产生期望地速大小
            if self._rest_speed_pid:
                self._rest_speed_pid = False
                self._gspeed_sp_pid.reset()

            # 增量式PID
            # self._gspeed_sp_pid.increment_pid(
            #     self.mix_err_Xk,
            #     self._mix_Xerr_params.mix_kp,
            #     self._mix_Xerr_params.mix_ki,
            #     self._mix_Xerr_params.mix_kd,
            # )
            # self.fw_sp.ground_speed = self._gspeed_sp_pid.get_full_output()

            # PID
            self._gspeed_sp_pid.set_pid_params(0.5, 0.00001, 0.001, 25)
            self._gspeed_sp_pid.pid_calculate(
                0,
                - self.mix_err_Xk,
                self._dt
            )
            self.fw_sp.ground_speed = self._gspeed_sp_pid.get_output()
 
            # rospy.loginfo("期望地速大小: {}".format(self.fw_sp.ground_speed))

            # 3. 根据期望地速产生期望空速
            self._fw_wind_vector.set_vec_ele(
                self.fw_state.wind_estimate_x, self.fw_state.wind_estimate_y
            )

            wind_Xk = fw_dir_unit.dot(self._fw_wind_vector)  # 沿着飞机机体前向的风估计

            self._airspeed_sp = self.fw_sp.ground_speed + wind_Xk

            # rospy.loginfo("原始空速设定值: {}".format(self._airspeed_sp))

            # 符合飞机本身的加减速特性
            # ATTENTION:慎用，加上之后,需要加大飞机的前向后相加速度，由于延时作用太强，可能导致不稳定。
            if (
                self._airspeed_sp - self._airspeed_sp_prev
            ) > self.fw_params.maxinc_acc * self._dt:
                self._airspeed_sp = (
                    self._airspeed_sp_prev + self.fw_params.maxinc_acc * self._dt
                )
            elif (
                self._airspeed_sp - self._airspeed_sp_prev
            ) < -self.fw_params.maxinc_acc * self._dt:
                self._airspeed_sp = (
                    self._airspeed_sp_prev - self.fw_params.maxinc_acc * self._dt
                )

            self._airspeed_sp = self._airspeed_sp_filter.update(
                self._airspeed_sp
            )  # 一阶低通滤波

            self._airspeed_sp_prev = self._airspeed_sp

            self.fw_sp.air_speed = constrain(
                self._airspeed_sp,
                self.fw_params.min_airspeed_sp,
                self.fw_params.max_airspeed_sp,
            )
            # rospy.loginfo("最终空速设定值: {}".format(self.fw_sp.air_speed))
        else:
            # 分段有误
            pass

        """
        8. 调用TECS获得期望俯仰角和期望油门
        """

        if self._rest_tecs:
            self._rest_tecs = False
            self._tecs.reset_states()

        # 设置参数，真实的飞机参数还需要实际调参
        self._tecs.set_speed_weight(self._tecs_params.speed_weight)
        self._tecs.set_time_const_throttle(
            self._tecs_params.time_const_throttle
        )  # 这个值影响到总能量——>油门，相当于Kp的倒数
        self._tecs.set_time_const(
            self._tecs_params.time_const
        )  # 这个值影响到能量分配——>俯仰角，相当于Kp的倒数

        self._tecs.enable_airspeed(True)

        # if self.fw_sp.altitude - self.fw_state.altitude >= 10:  # 判断一下是否要进入爬升
        if self.fw_sp.altitude - abs(self.fw_state.ned_pos_d) >= 10:  # 判断一下是否要进入爬升
            self._tecs_params.climboutdem = True
        else:
            self._tecs_params.climboutdem = False

        self._tecs.update_vehicle_state_estimates(
            self._fw_state_f.air_speed,
            self._fw_state_f.rotmat,
            self._fw_state_f.body_acc,
            self._fw_state_f.altitude_lock,
            self._fw_state_f.in_air,
            # self._fw_state_f.altitude, # 这里虚拟领导者用的是ned坐标系下的高度
            abs(self._fw_state_f.ned_pos_d), # 这里虚拟领导者用的是ned坐标系下的高度
            self._vz_valid,
            self._fw_state_f.ned_vel_d,
            self._fw_state_f.body_acc[2],
        )

        self._tecs.update_pitch_throttle(
            self.fw_state.rotmat,
            self._fw_state_f.pitch_angle,
            # self._fw_state_f.altitude,
            abs(self._fw_state_f.ned_pos_d), # 这里虚拟领导者用的是ned坐标系下的高度
            self.fw_sp.altitude, # 这里的altitude实际上ned坐标d的绝对值
            self.fw_sp.air_speed,
            self._fw_state_f.air_speed,
            self._tecs_params.EAS2TAS,
            self._tecs_params.climboutdem,
            self._tecs_params.climbout_pitch_min_rad,
            self.fw_params.throttle_min,
            self.fw_params.throttle_max,
            self.fw_params.cruise_throttle,
            self.fw_params.pitch_min_rad,
            self.fw_params.pitch_max_rad,
        )

        self.fw_cmd.pitch = self._tecs.get_pitch_setpoint()
        self.fw_cmd.throttle = self._tecs.get_throttle_setpoint()
        # print("TECS mode:", self._tecs.tecs_mode())

        """
        9. 调用L1控制器获得期望滚转角
        """

        if self.__controller_method == LONG_DIS:
            # if True: # 目前只考虑用L1控制器
            # L1 控制方法
            self._l1_controller.l1_controller(
                current_pos,
                pos_sp,
                self._fw_gspeed_2d,
                self._fw_state_f.air_speed,
            )

            roll_cmd = self._l1_controller.nav_roll()
        elif self.__controller_method == CLOSE_DIS:
            # elif False:
            # 编队位置与角度误差控制方法
            # if self.fw_error.led_fol_eta <= deg_2_rad(20.0) and self.fw_error.led_fol_eta >= deg_2_rad(-20.0):
                # rospy.loginfo("横向控制：混合误差")
                self._mix_Yerr_params.keta_p = 2
                self._mix_Yerr_params.kp_p = 0.08

                self.mix_err_Yk = self._mix_Yerr_params.keta_p * self.fw_error.led_fol_eta + self._mix_Yerr_params.kp_p * self.fw_error.PYk
                # rospy.loginfo("fw_error.PYk:%f",self.fw_error.PYk)
                # rospy.loginfo("led_fol_eta:%f",self.fw_error.led_fol_eta)
                # rospy.loginfo("mix_err_Yk:%f", self.mix_err_Yk)

                if self._reset_lateral_controller:
                    self._reset_lateral_controller = False
                    self._roll_sp_pid.reset()

                # 增量式PID
                # self._roll_sp_pid.increment_pid(self.mix_err_Yk, self._mix_Yerr_params.mix_kp, self._mix_Yerr_params.mix_ki, self._mix_Yerr_params.mix_kd)
                # phi_dot_sp = self._roll_sp_pid.get_full_output()
                
                # PID
                self._roll_sp_pid.set_pid_params(self._mix_Yerr_params.mix_kp, self._mix_Yerr_params.mix_ki, self._mix_Yerr_params.mix_kd, deg_2_rad(30))
                self._roll_sp_pid.pid_calculate(0, - self.mix_err_Yk, self._dt)
                phi_dot_sp = self._roll_sp_pid.get_output()
                
                roll_cmd = math.atan(phi_dot_sp * self._fw_gspeed_2d.len() / GRAVITY)
                # rospy.loginfo("phi_dot_sp:%f", phi_dot_sp)
                # rospy.loginfo("roll_cmd:%f", roll_cmd)
                

                self._reset_eta_2_roll_sp_pid = True

            # else:
            #     self._lateral_mode = 2
            #     rospy.loginfo("横向控制：速度角误差")
            #     if self._reset_eta_2_roll_sp_pid:
            #         self._reset_eta_2_roll_sp_pid = False
            #         self._eta_2_roll_sp_pid.reset()

            #     self._eta_2_roll_sp_pid.set_pid_params(1,0,0,deg_2_rad(30))
            #     self._eta_2_roll_sp_pid.pid_calculate(0, -self.fw_error.led_fol_eta, self._dt)
            #     roll_cmd = self._eta_2_roll_sp_pid.get_output()

            #     self._reset_lateral_controller = True    

        else:
            # 分段有误，无法计算期望滚转
            rospy.loginfo("分段有误，无法计算期望滚转角")
            pass

        roll_cmd = constrain(
            roll_cmd, -self.fw_params.roll_max_rad, self.fw_params.roll_max_rad
        )
        self.fw_cmd.roll = roll_cmd
        # rospy.loginfo(
        #     "roll_cmd: %f, pitch_cmd: %f, yaw_cmd: %f, throttle_cmd: %f",
        #     self.fw_cmd.roll,
        #     self.fw_cmd.pitch,
        #     self.fw_cmd.yaw,
        #     self.fw_cmd.throttle,
        # )
        
        mix_error_2D = Pose2D()
        mix_error_2D.x = self.mix_err_Xk
        mix_error_2D.y = self.mix_err_Yk
        self._mix_err_2D_pub.publish(mix_error_2D)
        
        # rospy.loginfo("fw_error.PXK:%f", self.fw_error.PXk)
        # rospy.loginfo("fw_error.PYk:%f", self.fw_error.PYk)
        # rospy.loginfo("led_fol_eta:%f", self.fw_error.led_fol_eta)
        # rospy.loginfo("mix_err_Xk:%f", self.mix_err_Xk)
        # rospy.loginfo("mix_err_Yk:%f", self.mix_err_Yk)
        # rospy.loginfo("roll_cmd:%f", roll_cmd)
        print("\n")

        # print("PXk: {}".format(self.fw_error.PXk))
        # print("PYk: {}".format(self.fw_error.PYk))
        # print("led_fol_vk:", self.fw_error.led_fol_vk)
        # print("led_fol_eta:", self.fw_error.led_fol_eta)
        # print("mix_err_Xk: {}".format(self.mix_err_Xk))
        # print("mix_err_Yk: {}".format(self.mix_err_Yk))

        # 将当前变量的值创建为一个新的 DataFrame
        # data = pd.DataFrame(
        #     {
        #         "fw_pos_x": [self.fw_state.ned_pos_x],
        #         "fw_pos_y": [self.fw_state.ned_pos_y],
        #         "led_pos_x": [self.leader_states.ned_pos_n],
        #         "led_pos_y": [self.leader_states.ned_pos_e],
        #         "PXk": [self.fw_error.PXk],
        #         "PYk": [self.fw_error.PYk],
        #         "led_fol_vk": [self.fw_error.led_fol_vk],
        #         "led_fol_eta": [self.fw_error.led_fol_eta],
        #         "mix_err_Xk": [self.mix_err_Xk],
        #         "mix_err_Yk": [self.mix_err_Yk],
        #         "ground_speed_sp": [self.fw_sp.ground_speed],
        #         "air_speed_sp": [self.fw_sp.air_speed],
        #         "roll_sp": [self.fw_cmd.roll],
        #         "fw_sp.altitude": [self.fw_sp.altitude],
        #         "fw_state.altitude": [self.fw_state.altitude]
        #     }
        # )
        
        # csv_filename = 'output_data.csv'
        # if not self.data_head:
        #     # 如果是第一次写入，写入表头
        #     data.to_csv(csv_filename, index=False, header=True)
        #     self.data_head = True
        # else:
        #     # 如果不是第一次写入，不写入表头
        #     data.to_csv(csv_filename, index=False, mode='a', header=False)